// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DriveBoard.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

using System.Runtime.CompilerServices;

[assembly: InternalsVisibleTo("Microsoft.Robotics.Hardware.EV4.UnitTests")]

namespace Microsoft.Robotics.Hardware.EV4
{
    using System.Collections.Generic;
    using System.Runtime.InteropServices;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// The DriveBoard class
    /// </summary>
    public class DriveBoard : ControlBoard
    {
        /// <summary>
        /// The drive board heartbeat data
        /// </summary>
        private DriveHeartbeat driveHeartBeat;

        /// <summary>
        /// The saved configuration 
        /// </summary>
        private DeviceConfiguration configEv4;

        /// <summary>
        /// Synchronization object for making heartbeat update thread safe.
        /// </summary>
        private object syncRoot;

        /// <summary>
        /// Initializes a new instance of the <see cref="DriveBoard" /> class.
        /// </summary>
        /// <param name="config">A DeviceConfiguration that specifies the IDeviceInfo details of each device</param>
        /// <param name="transport">The transport</param>
        /// <param name="protocol">The protocol</param>
        /// <param name="pollingIntervalMS">The polling interval in milliseconds</param>
        public DriveBoard(DeviceConfiguration config, ITransport transport, IProtocol protocol, int pollingIntervalMS)
            : base(transport, protocol, pollingIntervalMS)
        {
            this.syncRoot = new object();
            this.driveHeartBeat = new DriveHeartbeat();
            this.configEv4 = config;
        }

        /// <summary>
        /// Prepares the drive board for use
        /// </summary>
        public void InitializeDriveBoard()
        {
            this.SendGetUnitInfo();
            this.SendGetSensorInfo();
            this.KeepAlive();
        }

        /// <summary>
        /// Enables cliff detection
        /// </summary>
        /// <param name="enable">True if cliff detection is to be enabled</param>
        public void EnableCliffDetection(bool enable)
        {
            ICommand command = new SettableFlagCommand(CommandId.CliffDetectionEnable, enable);
            this.DispatchCommand(command);
        }

        /// <summary>
        /// Generates a Heartbeat state from a drive heartbeat
        /// </summary>
        /// <returns>The HeartbeatState</returns>
        internal HeartbeatState GenerateHeartbeatState()
        {
            lock (this.syncRoot)
            {
                HeartbeatState state = new HeartbeatState()
                    {
                        Accelerometers =
                            new List<AccelerometerState>()
                                {
                                    this.configEv4.Accelerometer.Generate(
                                        new Vector3(
                                        this.driveHeartBeat.AccelRightInG, 
                                        this.driveHeartBeat.AccelUpInG, 
                                        this.driveHeartBeat.AccelForwardInG), 
                                        this.driveHeartBeat.TimeStamp)
                                }, 
                        Batteries =
                            new List<BatteryState>()
                                {
                                    this.configEv4.Battery.Generate(
                                        this.driveHeartBeat.BatteryVoltageInmV, 
                                        (double)this.driveHeartBeat.BatteryTemperatureIn10ThsCelcius / 10, 
                                        0, 
                                        this.driveHeartBeat.BatteryCurrentInmA, 
                                        (this.driveHeartBeat.SensorStates & DriveSensorStates.BatteryCharging) != 0,
                                        this.driveHeartBeat.BatteryChargeInPercent, 
                                        this.driveHeartBeat.BatteryStatus, 
                                        this.driveHeartBeat.TimeStamp)
                                }, 
                        DifferentialDrives =
                            new List<DifferentialDriveState>()
                                {
                                    this.configEv4.Drive.Generate(
                                        this.driveHeartBeat.MotorRightVelocityInMperSec, 
                                        this.driveHeartBeat.MotorRightPositionInMm, 
                                        this.driveHeartBeat.MotorRightAccelerationInMperSec2, 
                                        this.driveHeartBeat.MotorLeftVelocityInMperSec, 
                                        this.driveHeartBeat.MotorLeftPositionInMm, 
                                        this.driveHeartBeat.MotorLeftAccelerationInMperSec2, 
                                        this.driveHeartBeat.TimeStamp,
                                        this.configEv4.WheelBaseInMeters)
                                }, 
                        Fans =
                            new List<FanState>()
                                {
                                    this.configEv4.LeftFan.Generate(
                                        0, this.driveHeartBeat.LeftFanRpm, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.RightFan.Generate(
                                        0, this.driveHeartBeat.RightFanRpm, this.driveHeartBeat.TimeStamp)
                                }, 
                        Gyroscopes =
                            new List<GyroscopeState>()
                                {
                                    this.configEv4.Gyroscope.Generate(
                                        new Vector3(
                                        this.driveHeartBeat.GyroRightInRadiansPerSecond, 
                                        this.driveHeartBeat.GyroUpInRadiansPerSecond, 
                                        this.driveHeartBeat.GyroForwardInRadiansPerSecond), 
                                        this.driveHeartBeat.TimeStamp)
                                }, 
                        HumiditySensors =
                            new List<HumidityState>()
                                {
                                    this.configEv4.HumiditySensor.Generate(
                                        this.driveHeartBeat.RelativeHumidityInPercent, this.driveHeartBeat.TimeStamp)
                                }, 
                        InfraredSensors =
                            new List<IRProximityState>()
                                {
                                    this.configEv4.IRFrontLeft.Generate(
                                        (double)this.driveHeartBeat.Prox2InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.IRFrontCenter.Generate(
                                        (double)this.driveHeartBeat.Prox4InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.IRFrontRight.Generate(
                                        (double)this.driveHeartBeat.Prox6InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.IRRearRight.Generate(
                                        (double)this.driveHeartBeat.Prox8InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.CliffFrontLeft.Generate(
                                        (double)this.driveHeartBeat.Cliff1InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.CliffFrontCenter.Generate(
                                        (double)this.driveHeartBeat.Cliff2InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.CliffFrontRight.Generate(
                                        (double)this.driveHeartBeat.Cliff3InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.CliffRearLeft.Generate(
                                        (double)this.driveHeartBeat.Cliff4InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.CliffRearRight.Generate(
                                        (double)this.driveHeartBeat.Cliff5InMm / 1000, this.driveHeartBeat.TimeStamp), 
                                }, 
                        Joints =
                            new List<JointState>()
                                {
                                    this.configEv4.Hood.Generate(
                                        0, 
                                        this.driveHeartBeat.CameraHeadingInRadians, 
                                        0, 
                                        0, 
                                        this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.Turret.Generate(
                                        0, 
                                        this.driveHeartBeat.TurretHeadingInRadians, 
                                        (double)this.driveHeartBeat.TurretVelocityInMiliRadsPerSec / 1000, 
                                        0, 
                                        this.driveHeartBeat.TimeStamp)
                                }, 
                        PowerDevices =
                            new List<PowerState>()
                                {
                                    this.configEv4.Power.Generate(
                                        (int)this.driveHeartBeat.PowerStates, this.driveHeartBeat.TimeStamp)
                                }, 
                        Projectors =
                            new List<ProjectorState>()
                                {
                                    this.configEv4.Projector.Generate(
                                        this.driveHeartBeat.ProjectorInversion != 0x00, 
                                        false, 
                                        this.driveHeartBeat.ProjectorFocusPositionInSteps, 
                                        (short)this.driveHeartBeat.ProjectorBrightness, 
                                        this.driveHeartBeat.TimeStamp)
                                }, 
                        Sonars =
                            new List<SonarState>()
                                {
                                    this.configEv4.SonarLeft.Generate(
                                        (double)this.driveHeartBeat.SonarDistanceLeftInMillimeters / 1000, 
                                        this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.SonarRight.Generate(
                                        (double)this.driveHeartBeat.SonarDistanceRightInMillimeters / 1000, 
                                        this.driveHeartBeat.TimeStamp), 
                                }, 
                        TemperatureSensors =
                            new List<TemperatureState>()
                                {
                                    this.configEv4.BatteryTemperature.Generate(
                                        (double)this.driveHeartBeat.BatteryTemperatureIn10ThsCelcius / 10, 
                                        this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.TemperatureInternal.Generate(
                                        (double)this.driveHeartBeat.TemperatureInternalIn10ThsCelsius / 10, 
                                        this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.TemperatureExternal.Generate(
                                        (double)this.driveHeartBeat.TemperatureExternalIn10ThsCelsius / 10, 
                                        this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.TemperatureProjectorBlue.Generate(
                                        this.driveHeartBeat.TemperatureProjectorBlueInCelsius, 
                                        this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.TemperatureProjectorRed.Generate(
                                        this.driveHeartBeat.TemperatureProjectorRedInCelsius, 
                                        this.driveHeartBeat.TimeStamp), 
                                    this.configEv4.TemperatureProjectorGreen.Generate(
                                        this.driveHeartBeat.TemperatureProjectorGreenInCelsius, 
                                        this.driveHeartBeat.TimeStamp), 
                                }, 
                    };

                return state;
            }
        }

        /// <summary>
        /// Deserializes a DriveHeartbeat from the raw buffer received from DriveBoard.
        /// The raw buffer contains a DriveHeartbeat as a buffer of bytes.
        /// </summary>
        /// <param name="buffer">The raw buffer containing the DriveHeartbeat as a byte array</param>
        /// <param name="offset">Offset into buffer</param>
        /// <param name="length">The parameter is not used.</param>
        /// <returns>A HeartbeatState</returns>
        protected override HeartbeatState ProcessHeartbeatData(byte[] buffer, int offset, int length)
        {
            lock (this.syncRoot)
            {
                Marshal.PtrToStructure(Marshal.UnsafeAddrOfPinnedArrayElement(buffer, offset), this.driveHeartBeat);
            }

            return this.GenerateHeartbeatState();
        }

        /// <summary>
        /// Sends a HostHeartbeatIgnore command to the drive board
        /// This removes the requirement to send regular host heartbeats
        /// </summary>
        private void SendHostHeartbeatIgnore()
        {
            this.DispatchCommand(new HostHeartbeatIgnore());
        }
    }
}
